Great,

1. I have added two test code at add test for end-coords-interpolation #325 #326
2. for head problem, I think it makes sense to remove :head, :torso from limbs. but set use-torso nil is too agressive, if you want we'd better to use option to change by users,
3. For rotation problem, I think the root problem is solving IK in (when end-coords-interpolation ;; set end-coords interpolation block, I still working on this, but for example if we change something like below, we can get better result

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   (send robot :angle-vector av-prev)

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   ;;(send robot :angle-vector av-prev)                            
                   ;;(send robot :angle-vector av)                                 
                   (if (car interpolated-avs)
                       (send robot :angle-vector (car interpolated-avs)))

will pass the tests, but would cause another problem...

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

1. Commented one change on the test code in add test for end-coords-interpolation #325 #326. Still fails normally and passes after changes or with

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   ;;(send robot :angle-vector av-prev)                            
                   ;;(send robot :angle-vector av)                                 
                   (if (car interpolated-avs)
                       (send robot :angle-vector (car interpolated-avs)))

2. Agree on making :use-torso be an option changeable by users. However, still believe setting it to nil should be better for avoiding unintended and/or fast torso movements.

On this topic, I have also thought about making the time-interval of the IK's (in this case 100) an option and/or passing &rest arguments to the IK call (like :thre). At the end I found it to be too many options for a rarely-used method and did not implement it, but what do you think about it? Is it worth it?

3. As another option, I have also tried to add an :additional-check to find a solution fairly close (or the closest among them) to the previous angle-vector. It looked something like this:

 (let ((min 1000)
        (origin-vec (send *pr2* :angle-vector))
        vec)
...
     (send *pr2* :inverse-kinematics end-lst :stop times :additional-check
           #'(lambda ()
               (let ((diff (norm (v- (send *pr2* :angle-vector) origin-vec))))
                 (if (< diff min) (setq vec (send *pr2* :angle-vector) min diff)) (< diff cthre))))
...

As a result I got a rather smooth path, but calculations took a long time and final angle-vector was pretty different from given one. However, without pr2-interface.l changes it stills draws a weird path, zig-zagging between the generated midpoint vectors.

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

@pazeshun used this function in ARC2017.

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

@pazeshun used this function in ARC2017.

Sorry for late.
https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/euslisp/lib/arc-interface.l#L778-L779

ok, I have updated https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/326/files, this will chose smoothest interpolation from different strategy, check if this works for @Affonso-Gui and did not break your current system @pazeshun

Agree on making :use-torso be an option changeable by users. However, still believe setting it to nil should be better for avoiding unintended and/or fast torso movements.

can you add this change to PR?

On this topic, I have also thought about making the time-interval of the IK's (in this case 100) an option and/or passing &rest arguments to the IK call (like :thre). At the end I found it to be too many options for a rarely-used method and did not implement it, but what do you think about it? Is it worth it?

Is 449446b ok?

As another option, I have also tried to add an :additional-check to find a solution fairly close (or the closest among them) to the previous angle-vector. It looked something like this:

Not sure if I understand correctly, does #326 solve your problem? since :inverse-kinematics is gradient based method, so if you start calculation from "previous angle-vector", then you'll get closest solution. If you want to "choose" from multiple solutions, you have to call ik from multiple initial point (We've seen this approach at @ishiguroJSK 's IROS report at lab meeting, which was noted as a not a good idea in Kajita-san's text book)

Interesting approach, but still had some problems when testing. Pointed out some of them on #326, some others are the ones already commented above.

1. Does not behave properly when over-360 degree turns are present. This is because pr2-interface splits the angle vector in its midpoints, which is not what we want to do in :end-coords-interpolation.
2. Move only in 50 ms when all end-coords are the same.
3. It actually does not send designed angle-vector in the end.

Also, it seems to be somewhat cost-expensive: it took me around 5 seconds to calculate the path for test-angle-vector-sequence-end-coords-interpolation.

can you add this change to PR?

Added :use-torso and :end-coords-interpolation-pass-time to this PR 6db0e99

Actually I do not believe that the main problem is the path not being smooth enough, but rather the fact that the trajectory gets split up in pr2-interface, before reaching robot-interface's :angle-vector-sequence.

Updated for

• use steps (number of divisions) instead of pass-time argument
• theoretically interpolate in given time.
• move accordingly for over 360 deg rotations

thank you for explanation, merged.